package com.hitqz.robot.biz.flow.component.patrol;

import cn.hutool.core.util.StrUtil;
import com.alibaba.fastjson.JSONObject;
import com.baomidou.mybatisplus.core.toolkit.Wrappers;
import com.hitqz.robot.api.business.enums.RobotOtherCommandEnum;
import com.hitqz.robot.api.business.enums.RobotParamsFormFieldEnum;
import com.hitqz.robot.api.business.model.dto.FlowContextDto;
import com.hitqz.robot.api.business.model.dto.FlowPatrolPreAndEndTicketsDto;
import com.hitqz.robot.api.common.dto.robot.DoorStatusDto;
import com.hitqz.robot.api.common.entity.FormFieldConfig;
import com.hitqz.robot.api.common.entity.RobotMapPos;
import com.hitqz.robot.api.common.entity.TicketsTask;
import com.hitqz.robot.biz.business.dispatch.DispatchService;
import com.hitqz.robot.biz.flow.config.DoorOpenException;
import com.hitqz.robot.biz.flow.util.LiteFlowUtil;
import com.hitqz.robot.biz.service.FormFieldConfigService;
import com.hitqz.robot.biz.service.RobotMapPosService;
import com.hitqz.robot.common.core.util.R;
import com.hitqz.robot.driver.model.IRobotPosDto;
import com.hitqz.robot.driver.parent.RobotDriverService;
import com.yomahub.liteflow.annotation.LiteflowComponent;
import com.yomahub.liteflow.core.NodeComponent;
import lombok.AllArgsConstructor;
import lombok.extern.slf4j.Slf4j;

import java.util.Objects;
import java.util.concurrent.TimeUnit;

@Slf4j
@LiteflowComponent(value = "GoIntoChargeHomeDoor", name = "[巡检]进门禁充电房")
@AllArgsConstructor
public class GoIntoChargeHomeDoor extends NodeComponent {

    private final RobotDriverService robotDriverService;


    private final RobotMapPosService robotMapPosService;

    private final DispatchService dispatchService;

    private final FormFieldConfigService formFieldConfigService;


    @Override
    public void process() throws Exception {
        FlowContextDto dto = this.getRequestData();
        TicketsTask task = dto.getTicketsTask();
        FlowPatrolPreAndEndTicketsDto params = JSONObject.parseObject(task.getParseOrderParams(), FlowPatrolPreAndEndTicketsDto.class);
        log.info("{} ---params: {}", task.getParseOrderParams(), JSONObject.toJSONString(params));
        if (StrUtil.isBlank(params.getPatrolItemIds())){
            log.warn("没有检测项信息,不执行");
            return;
        }
        FormFieldConfig insidePosConfig = formFieldConfigService.getOne(Wrappers.<FormFieldConfig>lambdaQuery().eq(FormFieldConfig::getFieldName, RobotParamsFormFieldEnum.CHARGE_HOME_INSIDE_DOOR_POS_CODE.getFieldName()).last("limit 1"));
        FormFieldConfig outSidePosConfig = formFieldConfigService.getOne(Wrappers.<FormFieldConfig>lambdaQuery().eq(FormFieldConfig::getFieldName, RobotParamsFormFieldEnum.CHARGE_HOME_OUTSIDE_DOOR_POS_CODE.getFieldName()).last("limit 1"));
        FormFieldConfig chargePosConfig = formFieldConfigService.getOne(Wrappers.<FormFieldConfig>lambdaQuery().eq(FormFieldConfig::getFieldName, RobotParamsFormFieldEnum.CHARGE_POS_CODE.getFieldName()).last("limit 1"));
        if (insidePosConfig==null || outSidePosConfig==null || chargePosConfig ==null){
            log.info("无充电房门禁配置");
            return;
        }
        String insidePosCode = insidePosConfig.getCurrentValue();
        String outsidePosCode = outSidePosConfig.getCurrentValue();
        String chargePosCode = chargePosConfig.getCurrentValue();
        if (StrUtil.isBlank(insidePosCode) || StrUtil.isBlank(outsidePosCode) || StrUtil.isBlank(chargePosCode)){
            log.info("未配置充电房门禁点编号");
            return;
        }
        RobotMapPos endPos = robotMapPosService.getById(params.getEndPosId());
        if ( !Objects.equals(endPos.getCode(), chargePosCode)){
            log.info("最后停留点和充电点不一致，无需控制门禁");
            return;
        }
        //获取当前位置
        IRobotPosDto iRobotPosDto = robotDriverService.getWebPos();
        //未在门禁外的点
        if ( !Objects.equals(iRobotPosDto.getCurrentStation(), outsidePosCode) || !iRobotPosDto.isReached()) {
            //调用规划路径算法的前往站点
            RobotMapPos outSidePos = robotMapPosService.getOne(Wrappers.<RobotMapPos>lambdaQuery().eq(RobotMapPos::getCode, outsidePosCode).last("limit 1"));
            log.info("前往门禁外点：{}", outsidePosCode);
            dispatchService.go2TargetPosition(outSidePos, false);
            TimeUnit.SECONDS.sleep(1);
            iRobotPosDto = robotDriverService.getWebPos();
            while ( !(Objects.equals(iRobotPosDto.getCurrentStation(), outSidePos.getCode()) && iRobotPosDto.isReached())) {
                iRobotPosDto = robotDriverService.getWebPos();
                TimeUnit.SECONDS.sleep(1);
                //外部中断标志
                LiteFlowUtil.checkInterruptFlag();
            }
        }
        log.info("到达门禁外点：{},执行开门", outsidePosCode);
        DoorStatusDto doorStatusDto = new DoorStatusDto();
        doorStatusDto.setSn(1);
        doorStatusDto.setStatus(1);
        doorStatusDto.setAsync(false);
        boolean openDoorSuccess = false;
        for (int i = 0; i < 3; i++) {
            R result = robotDriverService.executeOtherCommand(RobotOtherCommandEnum.SET_DOOR_STATUS, JSONObject.toJSONString(doorStatusDto));
            if (result.getCode() == R.failed().getCode()) {
                log.error("任务中开门失败,重试");
            }
            if (result.getCode() == R.ok().getCode()){
                openDoorSuccess = true;
                log.info("任务中开门成功");
                break;
            }
        }
        if (!openDoorSuccess){
            throw new DoorOpenException();
        }
        RobotMapPos insidePos = robotMapPosService.getOne(Wrappers.<RobotMapPos>lambdaQuery().eq(RobotMapPos::getCode, insidePosCode).last("limit 1"));
        log.info("前往门禁内点：{}", insidePosCode);
        dispatchService.go2TargetPosition(insidePos, false);
        boolean reachInsidPos = false;
        while ( !reachInsidPos) {
            iRobotPosDto = robotDriverService.getWebPos();
            double distance = Math.sqrt(Math.pow(insidePos.getPosX() - iRobotPosDto.getX(), 2) + Math.pow(insidePos.getPosY() - iRobotPosDto.getY(), 2));
            reachInsidPos = distance < 0.6d;
            TimeUnit.SECONDS.sleep(1);
            //外部中断标志
            LiteFlowUtil.checkInterruptFlag();
        }
        log.info("到达门禁内点：{},执行关门", outsidePosCode);
        doorStatusDto.setStatus(0);
        doorStatusDto.setAsync(false);
        robotDriverService.executeOtherCommand(RobotOtherCommandEnum.SET_DOOR_STATUS, JSONObject.toJSONString(doorStatusDto));

    }



}
